%***********************************************************
% CS 229 Machine Learning
% Project, Ground truth data generation
%-----------------------------------------------------------
% Date : November 13, 2010
%***********************************************************

clear all;

addpath(genpath('./KalmanAll/'))

%**************************************************************************
%Input parameters
outputwidth = 320;
outputheight = 180;
Wmax = 640;
Hmax = 360;
scale_conversion_factor_tracking = 1.5;

lecturer_tracking_scale_factor = 640/2560;
saliency_tracking_scale_factor = 640/1920;
board_scale_factor = 640/1920;

load groundTruth.dat;
load lecturer.dat;
load saliency.dat;
load boards.dat;
load predicted_panning_label.mat;
%**************************************************************************


dataA = groundTruth;

% The required frame rate is 33 ms per frame. Our data contains observations 
% that are taken at time intervals that are multiples of 100 ms. Thus, we 
% need to interpolate these observations. 

size(dataA)

%--------------------------------------------------------------------------
% Interpolate data
interpolatedTimeStamps = (dataA(1,1):100/3:dataA(size(dataA,1),1));
newDataA = zeros(size(interpolatedTimeStamps',1),3);
newDataA(:,1) = interpolatedTimeStamps';
newDataA(:,2) = (interp1(dataA(:,1)', dataA(:,2)', interpolatedTimeStamps))'./scale_conversion_factor_tracking+10;
newDataA(:,3) = (interp1(dataA(:,1)', dataA(:,3)', interpolatedTimeStamps))'./scale_conversion_factor_tracking+10;

%--------------------------------------------------------------------------
%Adjustment for camera border case
%newDataA(:,2) = min(max(floor(newDataA(:,2) - outputwidth/2),1),Wmax-outputwidth) + outputwidth/2;
%newDataA(:,3) = min(max(floor(newDataA(:,3) - outputheight/2),1),Hmax-outputheight) +outputheight/2 ;
%--------------------------------------------------------------------------

%==========================================================================================

y = newDataA(:,2);
y = smooth(y,150);

%calculate board center in x-direction
board_center_x = (boards(:,1) + boards(:,2))/2;
board_center_x = board_center_x*board_scale_factor;
% for i = 1:size(board_center_x,1)
%     board_center_x(i,:) = min(max(floor(board_center_x(i,:) - outputwidth/2),1),Wmax-outputwidth) + outputwidth/2;
% end
% Prepare features first

size(lecturer)
size(saliency)
size(y)

% Scale vectors
%lecuturer feature adjustment
lecturer = lecturer * lecturer_tracking_scale_factor; 
lecturer(:,2) = min(max(floor(lecturer(:,2) - outputwidth/2),1),Wmax-outputwidth) + outputwidth/2;
lecturer(:,3) = min(max(floor(lecturer(:,3) - outputheight/2),1),Hmax-outputheight) +outputheight/2 ;

%saliency feature adjustment
saliency = saliency *  saliency_tracking_scale_factor;
%saliency(:,2) = saliency(:,2) + outputwidth/2;
%saliency(:,3) = saliency(:,3) + outputheight/2;

minFrameOffset = 1;
minError = 1e999;
trainMSEArr = []; 
testMSEArr = []; 
frame_offset_arr = [];
cvMSE_arr = [];

bestMSE = 1e99;

% for frame_offset = 15900:100:15900 %% Prohibitively slow, so don't do more loops

    frame_offset = 15900;
    sample_step = max([1 round(frame_offset*0.1)]);
    training_seq = 1:30000;
    test_seq = 30001:50000;
    numPoints = 50000;

    %Prepare features
    X = PrepareFeatures(1, numPoints, frame_offset,sample_step, lecturer, saliency,board_center_x  );
    X = X';
    y = y(1:numPoints);

    %regf=@(XTRAIN,ytrain,XTEST)(XTEST*regress(ytrain,XTRAIN));

    %cross validation training
    regf=@modelTrainPred;
    cvMse = crossval('mse',X(training_seq,:),y(training_seq),'predfun',regf);

    %trainMSEArr = [trainMSEArr;  train_MSE]; 
    %testMSEArr = [testMSEArr;  MSE]; 

    if(cvMse<bestMSE)
        bestMSE = cvMse;
        bestb = glmfit(X(training_seq,:), y(training_seq), 'normal');
    end

    cvMSE_arr =  [ cvMSE_arr;cvMse; ];
    frame_offset_arr = [frame_offset_arr; frame_offset;];

% end

% figure(5)
% plot(frame_offset_arr,cvMSE_arr);
% legend('error');
% 
%calculate tracking and saliency errors
tracking_errors = y(test_seq) - lecturer(test_seq,2);
sal_errors = y(test_seq) - saliency(test_seq,2);
salMSE = mean(sum(sal_errors.*sal_errors))
trackingMSE = mean(sum(tracking_errors.*tracking_errors))

%predict on test set
testX = X(test_seq,:);
testOutput = [ones(size(testX,1),1) testX]*bestb;

errors =y(test_seq)-testOutput;
MSE_before_panning_filter = mean(sum(errors.*errors))

figure(88)
t=test_seq;
plot(t,testOutput,t,y(t),'LineWidth',1.8,'MarkerSize',8);
grid on;
%title({['Predicted and actual camera trajectory for'  num2str(numTestPoints) ' frames' ];['MSE: ' num2str(MSE)] });
fignam = 'Before panning decision';
saveas(gcf,[fignam '.eps'], 'psc2')
system( ['epstopdf ' fignam '.eps']);

% figure(89)
% plot(t, lecturer(1:numPoints,2), ...
%     t, saliency(1:numPoints,2),...
%     t,testOutput,t,y(1:numPoints));
% 
% title(['Errors: ' num2str(MSE) ' Sal Errors: ' num2str(salMSE) ' Tracking Errors: ' num2str(trackingMSE)]);
% legend('Before: Lecturer Traj','Saliency Traj','Estimiated Traj','Actual');


%find nearest board when panning state is off and move to it
numFrames = size(testOutput,1);
testOutput_original = zeros(size(testOutput, 1), 1);
testOutput_original(:,1) = testOutput(:,1);
for i=1:numFrames
    if(i > 1 && predicted_panning_label(i-1,1) == -1) %if panning state is negative
        testOutput(i,1) = testOutput(i-1,1); %if no pan, repeat last trajectory point
    elseif(predicted_panning_label(i,1) == -1) %first non-pan
        mindist = 1e99;
        for j=1:size(board_center_x,1)
            dist = 0;
            for k=1:min(300, i-1)             
                dist = dist + abs(board_center_x(j,1)-testOutput_original(i-k,1));
            end
            if(dist<mindist)
                mindist = dist;
                minbcx = board_center_x(j,1);
            end           
        end    
        testOutput(i,1) = minbcx;
    end
end

errors = y(test_seq)-testOutput;
MSE_after_panning_filter = mean(sum(errors.*errors))

% figure(2)
% plot(t, lecturer(1:numPoints,2), ...
%     t, saliency(1:numPoints,2),...
%     t,testOutput,t,y(1:numPoints));
% 
% title(['Errors: ' num2str(MSE) ' Sal Errors: ' num2str(salMSE) ' Tracking Errors: ' num2str(trackingMSE)]);
% legend('Lecturer Traj','Saliency Traj','Estimiated Traj','Actual');
% print('graph4.png', '-dpng')

figure(1)
plot(t,testOutput,t,y(t),'LineWidth',1.8,'MarkerSize',8);
grid on;
%title({['Predicted and actual camera trajectory for'  num2str(numTestPoints) ' frames' ];['MSE: ' num2str(MSE)] });
fignam = 'Prelim_learning_result';
saveas(gcf,[fignam '.eps'], 'psc2')
system( ['epstopdf ' fignam '.eps']);

%=======================================================================================================================
%Apply Smoothing

minMSE = 1e99;

for i = 250
testOutput_test = smooth(testOutput,i);
errors = y(test_seq)-testOutput_test;
MSE = mean(sum(errors.*errors));

if(MSE<minMSE)
    minMSE = MSE;
    bestsmoothpara= i;
end  
    
end

testOutput = smooth(testOutput,bestsmoothpara);
errors = y(test_seq)-testOutput;
MSE_final = mean(sum(errors.*errors))

% figure(3)
% plot(t, lecturer(1:numPoints,2), ...
%     t, saliency(1:numPoints,2),...
%     t,testOutput,t,y(1:numPoints));
% 
% title(['Errors: ' num2str(MSE) ' Sal Errors: ' num2str(salMSE) ' Tracking Errors: ' num2str(trackingMSE)]);
% legend('Lecturer Traj','Saliency Traj','Estimiated Traj','Actual');
% print('graph4.png', '-dpng')

figure(4)
plot(t,testOutput,t,y(t),'LineWidth',1.8,'MarkerSize',8);
grid on;
%title({['Predicted and actual camera trajectory for'  num2str(numTestPoints) ' frames' ];['MSE: ' num2str(MSE)] });
fignam = 'Prelim_learning_result';
saveas(gcf,[fignam '.eps'], 'psc2')
system( ['epstopdf ' fignam '.eps']);


% Final plot
figure(6)
plot(t,predicted_panning_label*50, t, y(t), t, testOutput);


